package pose;

import net.sf.json.JSONObject;

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import robot.Robot;


public class Pose {

	private Position position;
	private Orientation orientation;
	private long timestamp;
	private int status;

	public Pose() {
		super();
		position = new Position();
		orientation = new Orientation(0);
		timestamp = 0;
		status = 0;
	}

	public Pose(Position position, Orientation orientation, long timestamp,
			int status) {
		super();
		this.position = position;
		this.orientation = orientation;
		this.timestamp = timestamp;
		this.status = status;
	}

	public Position getPosition() {
		return position;
	}

	public void setPosition(Position position) {
		this.position = position;
	}

	public Orientation getOrientation() {
		return orientation;
	}

	public void setOrientation(Orientation orientation) {
		this.orientation = orientation;
	}

	public long getTimestamp() {
		return timestamp;
	}

	public void setTimestamp(long timestamp) {
		this.timestamp = timestamp;
	}

	public int getStatus() {
		return status;
	}

	public void setStatus(int status) {
		this.status = status;
	}
	
	/**
	 * Set attributes of Pose object from a JSON object
	 * @param obj the JSON Object to use
	 */
	public void setPoseFromJSONObject(JSONObject obj){
		position.setX(obj.getJSONObject("Pose").getJSONObject("Position").getDouble("X"));
		position.setY(obj.getJSONObject("Pose").getJSONObject("Position").getDouble("Y"));
		position.setZ(obj.getJSONObject("Pose").getJSONObject("Position").getDouble("Z"));
		orientation.setX(obj.getJSONObject("Pose").getJSONObject("Orientation").getDouble("X"));
		orientation.setY(obj.getJSONObject("Pose").getJSONObject("Orientation").getDouble("Y"));
		orientation.setZ(obj.getJSONObject("Pose").getJSONObject("Orientation").getDouble("Z"));
		orientation.setW(obj.getJSONObject("Pose").getJSONObject("Orientation").getDouble("W"));
	}

	public Vector3D convertPositionToMapCoordinateSystem(Orientation o){
		Rotation rot = new Rotation(o.getW(), o.getX(), o.getY(), o.getZ(), true);
		Vector3D positionInMap = rot.applyInverseTo(new Vector3D(position.getX(), position.getY(), position.getZ()));		
		return positionInMap;
	}
	
	/**
	 * Convert this point in the robot coordinate system 
	 * @param r the robot
	 * @return a vector which each component (x,y,z) is expressed is robot coordinate system
	 */
	public Vector3D convertPositionToRobotCoordinateSystem(Robot r){
		
		Vector3D positionInNewRepere = new Vector3D(position.getX() - r.getPose().getPosition().getX()
				, position.getY() - r.getPose().getPosition().getY(), position.getZ() - r.getPose().getPosition().getZ());
		double phi = r.getPose().getOrientation().convertQuarternionToEulerAngle().getZ();
		Vector3D postitionRelativeToRobot = new Vector3D(
													positionInNewRepere.getX()*Math.cos(phi) + positionInNewRepere.getY() * Math.sin(phi),
													-positionInNewRepere.getX()*Math.sin(phi) + positionInNewRepere.getY() * Math.cos(phi),
													positionInNewRepere.getZ()
											);
				
		return postitionRelativeToRobot;
	}

	@Override
	public String toString() {
		return "Pose [position=" + position + ", orientation=" + orientation
				+ ", timestamp=" + timestamp + ", status=" + status + "]";
	}
}
